/*
 * Robot.cpp
 *
 *  Created on: 23 янв. 2013
 *      Author: alex
 */

#define THRESHOLD 350

#include <cmath>
#include "Robot.h"
#include "Selector.h"
#include "khepera3.h"

Robot* Robot::_instance=0;

Robot::Robot()
{
	printf("khepera3_init\n");
	khepera3_init();
	khepera3_motor_initialize(&(khepera3.motor_left));
	khepera3_motor_initialize(&(khepera3.motor_right));

}

Robot::~Robot()
{
	khepera3_drive_set_speed(0,0);
}
Robot* Robot::instance()
{
	if(_instance==0)
		_instance = new Robot();
	return _instance;
}
static int dx[] = {1, 0, -1, 0}, dy[] = {0, 1, 0, -1};
void Robot::ReadSensors(double* dist)
{
    khepera3_infrared_proximity();

    //double dist[4]; //front,right,back,left;
    int *sensors = khepera3.infrared_proximity.sensor;
    printf("sensors: ");
    for(int i=0;i<11;i++)
    	printf("%d ",sensors[i]);
    printf("\n");

    dist[0] = (sensors[cKhepera3SensorsInfrared_FrontLeft] + sensors[cKhepera3SensorsInfrared_FrontRight]) / 2.0;
    dist[1] = sensors[cKhepera3SensorsInfrared_Right];
    dist[2] = sensors[cKhepera3SensorsInfrared_Back];
    dist[3] = sensors[cKhepera3SensorsInfrared_Left];
    //dist[0] = (sensors[eKhepera3SensorsInfrared] + sensors[6]) / 2.0;
    //    dist[1] = sensors[8];
    //    dist[2] = sensors[10];
    //    dist[3] = sensors[4];

}

void Robot::Tick()
{
	double dist[4];
    ReadSensors(dist);
    Point pos = Controller::instance()->Position();
        Tile tile = Controller::instance()->getTile();
        double wangle = pos.angle;
        if(wangle<0)
        	wangle = M_PI*2 + wangle;
        int shift = abs(int(wangle/M_PI_2+0.5));
        shift = shift%4;
        //if(shift==-1)shift=3;
        //else if(shift==-2) shift=2;
        printf("angle: %lf,wangle: %lf,shift: %d\n",pos.angle,wangle,shift);
        //double min=1000000;
        //int xmin,ymin;
        for(int i=0;i<4;i++)
    	{

    		int x = tile.x+dx[shift],y = tile.y+dy[shift];

    		shift++;
    		shift = shift%4;
    		if(x<0 || x>= OccupancyGrid::instance()->N() || y<0 || y >= OccupancyGrid::instance()->M())
    			continue;
    		//printf("proximity [%d %d]->%lf\n",x,y,dist[i]);
    		bool o = dist[i] > THRESHOLD;
    		//if(o)
    			//printf("obstacle found at %d %d\n",x,y);
    		OccupancyGrid::instance()->at(x,y,o);
//    		double v = OccupancyGrid::instance()->at(x,y);
  		printf("[%d %d]:prox=%lf\n",x,y,dist[i]);
//    		if(v<min)
//    		{
//    			min=v;
//    			xmin=x;ymin=y;
//    		}


    	}
        Tile dest = Selector::instance()->getDecision();
        fprintf(stderr,"go to tile %d %d\n",dest.x,dest.y);
        AttractivenessMap::instance()->at(dest.x,dest.y,true);
        Controller::instance()->goTo(dest);

}

